#include <stdio.h>
#include <stdlib.h>
#include <sys/time.h>

#include "camera.h"

#include "bcm_host.h"
#include "interface/vcos/vcos.h"

#include "interface/mmal/mmal.h"
#include "interface/mmal/util/mmal_default_components.h"
#include "interface/mmal/util/mmal_connection.h"

#define MMAL_CAMERA_PREVIEW_PORT 0
#define MMAL_CAMERA_VIDEO_PORT 1
#define MMAL_CAMERA_CAPTURE_PORT 2

#define IMAGE_WIDTH 1280
#define IMAGE_HEIGHT 720

#define ICV_WARP_CLIP_X(x)      ((unsigned)(x) < IMAGE_WIDTH ? (x) : (x) < 0 ? 0 : IMAGE_WIDTH - 1)
#define ICV_WARP_CLIP_Y(y)      ((unsigned)(y) < IMAGE_HEIGHT ? (y) : (y) < 0 ? 0 : IMAGE_HEIGHT - 1)

#define I420_PIXEL_REMAP_1280x720_Y(x, i, j)			((x) = ((i) << 10) + ((i) << 8) + (j))
#define I420_PIXEL_REMAP_1280x720_U_OR_V(x, i, j)		((x) = (((i) >> 1) << 9) + (((i) >> 1) << 7) + ((j) >> 1))

MMAL_POOL_T *camera_video_port_pool;
MMAL_POOL_T *preview_input_port_pool;
MMAL_PORT_T *preview_input_port = NULL;

#define DATA_NUMBER IMAGE_WIDTH*IMAGE_HEIGHT
#define LR_LENGTH 17

char video_proc_buf[1280 * 720 * 3 / 2];
volatile int is_video_under_proc = 0;
uint32_t video_frame_length;


char cLineBuf[LR_LENGTH];
int nWarpData[DATA_NUMBER][2];
int nYOffsetAddr = 0;
int nUOffsetAddr = 1280 * 720;
int nVOffsetAddr = (1280 * 720 * 5) / 4;
int nMaxX = 0;
int nMaxY = 0;

void analyse(char *buf, int idx)
{
    int i = 0;
    int j = 0;
    int nTmpNum[10];
    char *p;
    int nNumBuf = 0;
    int nFlagNeg = 0;
    for ( p = buf; *p != '\r'; p++ )
    {
        if ( *p == ',' )
        {
			for ( j = 0; j < i; j++)
			{
				nNumBuf += nTmpNum[j];
				if (j < i - 1)
				{
					nNumBuf *= 10;
				}
			}
			nWarpData[idx][0] = nNumBuf * (nFlagNeg ? (-1) : 1);
			i = 0;
			nNumBuf = 0;
        }
        else if( *p == '-' )
		{
			nFlagNeg = 1;
		}
        else
		{
			++i;
			nTmpNum[i-1] = *p - '0';
		}
    }

    for ( j = 0; j < i; j++)
    {
		nNumBuf += nTmpNum[j];
		if (j < i - 1)
		{
			nNumBuf *= 10;
		}
    }
    nWarpData[idx][1] = nNumBuf * (nFlagNeg ? (-1) : 1);
	if ( nWarpData[idx][1] > nMaxY )
	{
		nMaxY = nWarpData[idx][1];
		nMaxX = idx;
	}
}

void warp_data_init()
{
    FILE *fp;
    fp = fopen( "WarpData.txt", "r" );

    if ( fp == NULL )
    {
        printf( "File Open Error!\n" );
        return;
    }

    int nLineIdx;
    for ( nLineIdx = 0; nLineIdx < DATA_NUMBER; nLineIdx++ )
    {
        fgets(cLineBuf, LR_LENGTH, fp);
		if ( (nLineIdx % 10240) == 0 )
		{
			 printf("=");
		}
		analyse(cLineBuf, nLineIdx);
        //printf("%d, %d\n", nWarpData[nLineIdx][0], nWarpData[nLineIdx][1]);

    }
    printf("\nImporting Initial Warp Data From .txt File Completed!\n");
	printf("MaxX: %d\t MaxY: %d\n", nMaxX, nMaxY);
}

static void preview_buffer_callback(MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *buffer)
{
    //printf("INFO:preview_buffer_callback buffer->length = %d\n", buffer->length);

    mmal_buffer_header_release(buffer);
}

static void video_buffer_callback(MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *buffer)
{
	MMAL_BUFFER_HEADER_T *new_buffer;
	MMAL_POOL_T *pool = (MMAL_POOL_T *) port->userdata;

	if(is_video_under_proc == 0)
	{
		is_video_under_proc = 1;
		memcpy(video_proc_buf, buffer->data, 1280 * 720 * 3 / 2);
		video_frame_length = buffer->length;
	}

    mmal_buffer_header_release(buffer);

    // and send one back to the port (if still open)
    if (port->is_enabled)
	{
        MMAL_STATUS_T status;

        new_buffer = mmal_queue_get(pool->queue);

        if (new_buffer)
            status = mmal_port_send_buffer(port, new_buffer);

        if (!new_buffer || status != MMAL_SUCCESS)
            printf("Unable to return a buffer to the video port\n");
    }
}

int camera_initialize()
{
    MMAL_COMPONENT_T *camera = 0;
    MMAL_COMPONENT_T *preview = 0;
    MMAL_ES_FORMAT_T *format;
    MMAL_STATUS_T status;
    MMAL_PORT_T *camera_preview_port = NULL, *camera_video_port = NULL, *camera_still_port = NULL;



    MMAL_CONNECTION_T *camera_preview_connection = 0;

    printf("Running...\n");

	warp_data_init();

    bcm_host_init();

    status = mmal_component_create(MMAL_COMPONENT_DEFAULT_CAMERA, &camera);
    if (status != MMAL_SUCCESS)
	{
        printf("Error: create camera %x\n", status);
        return -1;
    }

    camera_preview_port = camera->output[MMAL_CAMERA_PREVIEW_PORT];
    camera_video_port = camera->output[MMAL_CAMERA_VIDEO_PORT];
    camera_still_port = camera->output[MMAL_CAMERA_CAPTURE_PORT];

    {
        MMAL_PARAMETER_CAMERA_CONFIG_T cam_config =
		{
            { MMAL_PARAMETER_CAMERA_CONFIG, sizeof (cam_config)},
            .max_stills_w = 1280,
            .max_stills_h = 720,
            .stills_yuv422 = 0,
            .one_shot_stills = 1,
            .max_preview_video_w = 1280,
            .max_preview_video_h = 720,
            .num_preview_video_frames = 3,
            .stills_capture_circular_buffer_height = 0,
            .fast_preview_resume = 0,
            .use_stc_timestamp = MMAL_PARAM_TIMESTAMP_MODE_RESET_STC
        };
        mmal_port_parameter_set(camera->control, &cam_config.hdr);
    }

    // Setup camera preview port format
    format = camera_preview_port->format;

    format->encoding = MMAL_ENCODING_OPAQUE;
    format->encoding_variant = MMAL_ENCODING_I420;

    format->es->video.width = 1280;
    format->es->video.height = 720;
    format->es->video.crop.x = 0;
    format->es->video.crop.y = 0;
    format->es->video.crop.width = 1280;
    format->es->video.crop.height = 720;

    status = mmal_port_format_commit(camera_preview_port);

    if (status != MMAL_SUCCESS)
	{
        printf("Error: camera viewfinder format couldn't be set\n");
        return -1;
    }

    // Setup camera video port format
    //mmal_format_copy(camera_video_port->format, camera_preview_port->format);

    format = camera_video_port->format;

    format->encoding = MMAL_ENCODING_I420;
    format->encoding_variant = MMAL_ENCODING_I420;

    format->es->video.width = 1280;
    format->es->video.height = 720;
    format->es->video.crop.x = 0;
    format->es->video.crop.y = 0;
    format->es->video.crop.width = 1280;
    format->es->video.crop.height = 720;
    format->es->video.frame_rate.num = 30;
    format->es->video.frame_rate.den = 1;


    camera_video_port->buffer_size = camera_video_port->buffer_size_recommended;
    camera_video_port->buffer_num = 2;

    status = mmal_port_format_commit(camera_video_port);

    printf(" camera video buffer_size = %d\n", camera_video_port->buffer_size);
    printf(" camera video buffer_num = %d\n", camera_video_port->buffer_num);
    if (status != MMAL_SUCCESS)
	{
        printf("Error: unable to commit camera video port format (%u)\n", status);
        return -1;
    }

    // crate pool form camera video port
    camera_video_port_pool = (MMAL_POOL_T *)mmal_port_pool_create(camera_video_port, camera_video_port->buffer_num, camera_video_port->buffer_size);
    camera_video_port->userdata = (struct MMAL_PORT_USERDATA_T *) camera_video_port_pool;

    status = mmal_port_enable(camera_video_port, video_buffer_callback);
    if (status != MMAL_SUCCESS)
	{
        printf("Error: unable to enable camera video port (%u)\n", status);
        return -1;
    }

    status = mmal_component_enable(camera);


    status = mmal_component_create(MMAL_COMPONENT_DEFAULT_VIDEO_RENDERER, &preview);
    if (status != MMAL_SUCCESS)
	{
        printf("Error: unable to create preview (%u)\n", status);
        return -1;
    }

    preview_input_port = preview->input[0];
    {
        MMAL_DISPLAYREGION_T param;
        param.hdr.id = MMAL_PARAMETER_DISPLAYREGION;
        param.hdr.size = sizeof (MMAL_DISPLAYREGION_T);
        param.set = MMAL_DISPLAY_SET_LAYER;
        param.layer = 0;
        param.set |= MMAL_DISPLAY_SET_FULLSCREEN;
        param.fullscreen = 1;
        status = mmal_port_parameter_set(preview_input_port, &param.hdr);
        if (status != MMAL_SUCCESS && status != MMAL_ENOSYS)
		{
            printf("Error: unable to set preview port parameters (%u)\n", status);
            return -1;
        }
    }
    mmal_format_copy(preview_input_port->format, camera_video_port->format);

    format = preview_input_port->format;

    format->encoding = MMAL_ENCODING_I420;
    format->encoding_variant = MMAL_ENCODING_I420;

    format->es->video.width = 1280;
    format->es->video.height = 720;
    format->es->video.crop.x = 0;
    format->es->video.crop.y = 0;
    format->es->video.crop.width = 1280;
    format->es->video.crop.height = 720;
    format->es->video.frame_rate.num = 30;
    format->es->video.frame_rate.den = 1;

    preview_input_port->buffer_size = camera_video_port->buffer_size_recommended;
    preview_input_port->buffer_num = 4;

    printf(" preview buffer_size = %d\n", preview_input_port->buffer_size);
    printf(" preview buffer_num = %d\n", preview_input_port->buffer_num);

    status = mmal_port_format_commit(preview_input_port);

    preview_input_port_pool = (MMAL_POOL_T *)mmal_port_pool_create(preview_input_port, preview_input_port->buffer_num, preview_input_port->buffer_size);

    preview_input_port->userdata = (struct MMAL_PORT_USERDATA_T *) preview_input_port_pool;
    status = mmal_port_enable(preview_input_port, preview_buffer_callback);
    if (status != MMAL_SUCCESS)
	{
        printf("Error: unable to enable preview input port (%u)\n", status);
        return -1;
    }

    /*
    status = mmal_connection_create(&camera_preview_connection, camera_preview_port, preview_input_port, MMAL_CONNECTION_FLAG_TUNNELLING | MMAL_CONNECTION_FLAG_ALLOCATION_ON_INPUT);
    if (status != MMAL_SUCCESS) {
        printf("Error: unable to create connection (%u)\n", status);
        return -1;
    }

    status = mmal_connection_enable(camera_preview_connection);
    if (status != MMAL_SUCCESS) {
        printf("Error: unable to enable connection (%u)\n", status);
        return -1;
    }
     */

	//Initialize the connection
    if (1)
	{
        // Send all the buffers to the encoder output port
        int num = mmal_queue_length(camera_video_port_pool->queue);
        int q;

        for (q = 0; q < num; q++)
		{
            MMAL_BUFFER_HEADER_T *buffer = mmal_queue_get(camera_video_port_pool->queue);

            if (!buffer)
                printf("Unable to get a required buffer %d from pool queue\n", q);

            if (mmal_port_send_buffer(camera_video_port, buffer) != MMAL_SUCCESS)
                printf("Unable to send a buffer to encoder output port (%d)\n", q);
        }


    }

    //printf("Camera module initialization completed!\r\n");

	//Fire the camera!
    if (mmal_port_parameter_set_boolean(camera_video_port, MMAL_PARAMETER_CAPTURE, 1) != MMAL_SUCCESS)
	{
        printf("%s: Failed to start capture\n", __func__);
    }
    return 0;
}


int camera_poll()
{
    if(is_video_under_proc == 1)
	{
		static int loop = 0;
		static struct timespec t1;
		struct timespec t2;
		//printf("INFO:video_buffer_callback\n");
		if (loop == 0)
		{
			clock_gettime(CLOCK_MONOTONIC, &t1);
		}
		clock_gettime(CLOCK_MONOTONIC, &t2);

		int d = t2.tv_sec - t1.tv_sec;

		MMAL_BUFFER_HEADER_T *preview_new_buffer;


		loop++;
		preview_new_buffer = mmal_queue_get(preview_input_port_pool->queue);

		if (preview_new_buffer)
		{
			//printf("preview_new_buffer->length = %d \n", preview_new_buffer->length);
			//printf("buffer->length = %d \n", buffer->length);
			//memcpy(preview_new_buffer->data, buffer->data, buffer->length);
			/*
			__asm__ __volatile__
			(
				""
			);
			*/
			char* src = video_proc_buf;
			char* dest = &(preview_new_buffer->data[0]);

			unsigned int i, j, k, h;
			unsigned int kk, hh;
			for( i = 0; i < 720; i++)
			{
				for( j = 0; j < 1280; j++ )
				{
					I420_PIXEL_REMAP_1280x720_Y(k, i, j);
					I420_PIXEL_REMAP_1280x720_U_OR_V(h, i, j);

					if( ((unsigned)nWarpData[k][0] < (IMAGE_WIDTH - 1)) && //
						((unsigned)nWarpData[k][1] < (IMAGE_HEIGHT - 1)) ) //J1
					{
						I420_PIXEL_REMAP_1280x720_Y(kk, nWarpData[k][1], nWarpData[k][0]);
						I420_PIXEL_REMAP_1280x720_U_OR_V(hh, nWarpData[k][1], nWarpData[k][0]);
						dest[nYOffsetAddr + k] = src[nYOffsetAddr + kk];
						dest[nUOffsetAddr + h] = src[nUOffsetAddr + hh];
						dest[nVOffsetAddr + h] = src[nVOffsetAddr + hh];
					}
					else if( ((unsigned)(nWarpData[k][0] + 1) < (IMAGE_WIDTH + 1)) && //J3
						((unsigned)(nWarpData[k][1] + 1) < (IMAGE_HEIGHT + 1)) )       //J4
					{
						unsigned int x0 = ICV_WARP_CLIP_X( nWarpData[k][0] );
						unsigned int y0 = ICV_WARP_CLIP_Y( nWarpData[k][1] );

						I420_PIXEL_REMAP_1280x720_Y(kk, y0, x0);
						I420_PIXEL_REMAP_1280x720_U_OR_V(hh, y0, x0);

						dest[nYOffsetAddr + k] = src[nYOffsetAddr + kk];
						dest[nUOffsetAddr + h] = src[nUOffsetAddr + hh];
						dest[nVOffsetAddr + h] = src[nVOffsetAddr + hh];
					}
					else//J6
					{
						dest[nYOffsetAddr + k] = 0x00;
					}
				}
			}

			/*
			int i;
			//921600 = 1280 * 720
			//set U = 0
			for (i=921600; i < 921600+230400; i++)
			{
				preview_new_buffer->data[i] = 0x00;
			}

			//set V = 0
			for (i=921600+230400; i < 1382400; i++)
			{
				preview_new_buffer->data[i] = 0b10101010;
			}
			*/
			preview_new_buffer->length = video_frame_length;

            comm_send(NULL, preview_new_buffer->data, preview_new_buffer->length);
            //comm_send(NULL, preview_new_buffer->data, 100);

			if (mmal_port_send_buffer(preview_input_port, preview_new_buffer) != MMAL_SUCCESS)
			{
				printf("ERROR: Unable to send buffer \n");
			}

			is_video_under_proc = 0;
		}
		else
		{
			printf("ERROR: mmal_queue_get (%d)\n", preview_new_buffer);
		}

		if (loop % 10 == 0)
		{
			//fprintf(stderr, "loop = %d \n", loop);
			if ( d != 0)
			{
				printf("loop = %d, Framerate = %f fps, buffer->length = %d \n", loop, loop / (float)(d), video_frame_length);
			}
		}
	}
}


int camera_finalize()
{

}



